using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;

namespace LightCAD.Three
{
    public class Matrix4 : IIOArray
    {
        #region scope properties or methods
        //private static Vector3 _v1 = new Vector3();
        //private static Matrix4 _m1 = new Matrix4();
        //private static Vector3 _zero = new Vector3(0, 0, 0);
        //private static Vector3 _one = new Vector3(1, 1, 1);
        //private static Vector3 _x = new Vector3();
        //private static Vector3 _y = new Vector3();
        //private static Vector3 _z = new Vector3();
        private static Matrix4Context getContext()
            => ThreadContext.getCurrThreadContext().Matrix4Ctx;
        #endregion

        #region Properties

        public double[] elements;

        #endregion

        #region constructor
        public Matrix4()
        {
            this.elements = new double[]{
                    1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, 0,
                    0, 0, 0, 1
                };
        }
        #endregion

        #region methods
        public Matrix4 set(double n11, double n12, double n13, double n14, double n21, double n22, double n23, double n24, double n31, double n32, double n33, double n34, double n41, double n42, double n43, double n44)
        {
            double[] te = this.elements;
            te[0] = n11; te[4] = n12; te[8] = n13; te[12] = n14;
            te[1] = n21; te[5] = n22; te[9] = n23; te[13] = n24;
            te[2] = n31; te[6] = n32; te[10] = n33; te[14] = n34;
            te[3] = n41; te[7] = n42; te[11] = n43; te[15] = n44;
            return this;
        }
        public bool isIdentity()
        {
            double[] te = this.elements;
            return te[0] == 1 && te[4] == 0 && te[8] == 0 && te[12] == 0 &&
                   te[1] == 0 && te[5] == 1 && te[9] == 0 && te[13] == 0 &&
                   te[2] == 0 && te[6] == 0 && te[10] == 1 && te[14] == 0 &&
                   te[3] == 0 && te[7] == 0 && te[11] == 0 && te[15] == 1;

        }
        public Matrix4 identity()
        {
            this.set(
                1, 0, 0, 0,
                0, 1, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 clone()
        {
            return new Matrix4().fromArray(this.elements);
        }
        public Matrix4 copy(Matrix4 m)
        {
            double[] te = this.elements;
            double[] me = m.elements;
            te[0] = me[0]; te[1] = me[1]; te[2] = me[2]; te[3] = me[3];
            te[4] = me[4]; te[5] = me[5]; te[6] = me[6]; te[7] = me[7];
            te[8] = me[8]; te[9] = me[9]; te[10] = me[10]; te[11] = me[11];
            te[12] = me[12]; te[13] = me[13]; te[14] = me[14]; te[15] = me[15];
            return this;
        }
        public Matrix4 copyPosition(Matrix4 m)
        {
            double[] te = this.elements, me = m.elements;
            te[12] = me[12];
            te[13] = me[13];
            te[14] = me[14];
            return this;
        }
        public Matrix4 setFromMatrix3(Matrix3 m)
        {
            double[] me = m.elements;
            this.set(
                me[0], me[3], me[6], 0,
                me[1], me[4], me[7], 0,
                me[2], me[5], me[8], 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 extractBasis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
        {
            xAxis.setFromMatrixColumn(this, 0);
            yAxis.setFromMatrixColumn(this, 1);
            zAxis.setFromMatrixColumn(this, 2);
            return this;
        }
        public Matrix4 makeBasis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
        {
            this.set(
                xAxis.x, yAxis.x, zAxis.x, 0,
                xAxis.y, yAxis.y, zAxis.y, 0,
                xAxis.z, yAxis.z, zAxis.z, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 extractRotation(Matrix4 m)
        {
            var ctx = getContext();
            var _v1 = ctx._v1;
            // this method does not support reflection matrices
            double[] te = this.elements;
            double[] me = m.elements;
            double scaleX = 1 / _v1.setFromMatrixColumn(m, 0).length();
            double scaleY = 1 / _v1.setFromMatrixColumn(m, 1).length();
            double scaleZ = 1 / _v1.setFromMatrixColumn(m, 2).length();
            te[0] = me[0] * scaleX;
            te[1] = me[1] * scaleX;
            te[2] = me[2] * scaleX;
            te[3] = 0;
            te[4] = me[4] * scaleY;
            te[5] = me[5] * scaleY;
            te[6] = me[6] * scaleY;
            te[7] = 0;
            te[8] = me[8] * scaleZ;
            te[9] = me[9] * scaleZ;
            te[10] = me[10] * scaleZ;
            te[11] = 0;
            te[12] = 0;
            te[13] = 0;
            te[14] = 0;
            te[15] = 1;
            return this;
        }
        public Matrix4 makeRotationFromEuler(Euler euler)
        {
            double[] te = this.elements;
            double x = euler.x, y = euler.y, z = euler.z;
            double a = JMath.cos(x), b = JMath.sin(x);
            double c = JMath.cos(y), d = JMath.sin(y);
            double e = JMath.cos(z), f = JMath.sin(z);
            if (euler.order == Euler.RotationOrders.XYZ)
            {
                double ae = a * e, af = a * f, be = b * e, bf = b * f;
                te[0] = c * e;
                te[4] = -c * f;
                te[8] = d;
                te[1] = af + be * d;
                te[5] = ae - bf * d;
                te[9] = -b * c;
                te[2] = bf - ae * d;
                te[6] = be + af * d;
                te[10] = a * c;
            }
            else if (euler.order == Euler.RotationOrders.YXZ)
            {
                double ce = c * e, cf = c * f, de = d * e, df = d * f;
                te[0] = ce + df * b;
                te[4] = de * b - cf;
                te[8] = a * d;
                te[1] = a * f;
                te[5] = a * e;
                te[9] = -b;
                te[2] = cf * b - de;
                te[6] = df + ce * b;
                te[10] = a * c;
            }
            else if (euler.order == Euler.RotationOrders.ZXY)
            {
                double ce = c * e, cf = c * f, de = d * e, df = d * f;
                te[0] = ce - df * b;
                te[4] = -a * f;
                te[8] = de + cf * b;
                te[1] = cf + de * b;
                te[5] = a * e;
                te[9] = df - ce * b;
                te[2] = -a * d;
                te[6] = b;
                te[10] = a * c;
            }
            else if (euler.order == Euler.RotationOrders.ZYX)
            {
                double ae = a * e, af = a * f, be = b * e, bf = b * f;
                te[0] = c * e;
                te[4] = be * d - af;
                te[8] = ae * d + bf;
                te[1] = c * f;
                te[5] = bf * d + ae;
                te[9] = af * d - be;
                te[2] = -d;
                te[6] = b * c;
                te[10] = a * c;
            }
            else if (euler.order == Euler.RotationOrders.YZX)
            {
                double ac = a * c, ad = a * d, bc = b * c, bd = b * d;
                te[0] = c * e;
                te[4] = bd - ac * f;
                te[8] = bc * f + ad;
                te[1] = f;
                te[5] = a * e;
                te[9] = -b * e;
                te[2] = -d * e;
                te[6] = ad * f + bc;
                te[10] = ac - bd * f;
            }
            else if (euler.order == Euler.RotationOrders.XZY)
            {
                double ac = a * c, ad = a * d, bc = b * c, bd = b * d;
                te[0] = c * e;
                te[4] = -f;
                te[8] = d * e;
                te[1] = ac * f + bd;
                te[5] = a * e;
                te[9] = ad * f - bc;
                te[2] = bc * f - ad;
                te[6] = b * e;
                te[10] = bd * f + ac;
            }
            // bottom row
            te[3] = 0;
            te[7] = 0;
            te[11] = 0;
            // last column
            te[12] = 0;
            te[13] = 0;
            te[14] = 0;
            te[15] = 1;
            return this;
        }

        internal void decompose(Vector3 position, Euler rotation, Vector3 scale)
        {
            throw new NotImplementedException();
        }

        public Matrix4 makeRotationFromQuaternion(Quaternion q)
        {
            var ctx = getContext();
            return this.compose(ctx._zero, q, ctx._one);
        }
        public Matrix4 lookAt(Vector3 eye, Vector3 target, Vector3 up)
        {
            var ctx = getContext();
            var _z = ctx._z;
            var _x = ctx._x;
            var _y = ctx._y;
            double[] te = this.elements;
            _z.subVectors(eye, target);
            if (_z.lengthSq() == 0)
            {
                // eye and target are in the same position
                _z.z = 1;
            }
            _z.normalize();
            _x.crossVectors(up, _z);
            if (_x.lengthSq() == 0)
            {
                // up and z are parallel
                if (JMath.abs(up.z) == 1)
                {
                    _z.x += 0.0001;
                }
                else
                {
                    _z.z += 0.0001;
                }
                _z.normalize();
                _x.crossVectors(up, _z);
            }
            _x.normalize();
            _y.crossVectors(_z, _x);
            te[0] = _x.x; te[4] = _y.x; te[8] = _z.x;
            te[1] = _x.y; te[5] = _y.y; te[9] = _z.y;
            te[2] = _x.z; te[6] = _y.z; te[10] = _z.z;
            return this;
        }
        public Matrix4 multiply(Matrix4 m)
        {
            return this.multiplyMatrices(this, m);
        }
        public Matrix4 premultiply(Matrix4 m)
        {
            return this.multiplyMatrices(m, this);
        }
        public Matrix4 multiplyMatrices(Matrix4 a, Matrix4 b)
        {
            double[] ae = a.elements;
            double[] be = b.elements;
            double[] te = this.elements;
            double a11 = ae[0], a12 = ae[4], a13 = ae[8], a14 = ae[12];
            double a21 = ae[1], a22 = ae[5], a23 = ae[9], a24 = ae[13];
            double a31 = ae[2], a32 = ae[6], a33 = ae[10], a34 = ae[14];
            double a41 = ae[3], a42 = ae[7], a43 = ae[11], a44 = ae[15];
            double b11 = be[0], b12 = be[4], b13 = be[8], b14 = be[12];
            double b21 = be[1], b22 = be[5], b23 = be[9], b24 = be[13];
            double b31 = be[2], b32 = be[6], b33 = be[10], b34 = be[14];
            double b41 = be[3], b42 = be[7], b43 = be[11], b44 = be[15];
            te[0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41;
            te[4] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42;
            te[8] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
            te[12] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;
            te[1] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
            te[5] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
            te[9] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
            te[13] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;
            te[2] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
            te[6] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
            te[10] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
            te[14] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;
            te[3] = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41;
            te[7] = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42;
            te[11] = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43;
            te[15] = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44;
            return this;
        }
        public Matrix4 multiplyScalar(double s)
        {
            double[] te = this.elements;
            te[0] *= s; te[4] *= s; te[8] *= s; te[12] *= s;
            te[1] *= s; te[5] *= s; te[9] *= s; te[13] *= s;
            te[2] *= s; te[6] *= s; te[10] *= s; te[14] *= s;
            te[3] *= s; te[7] *= s; te[11] *= s; te[15] *= s;
            return this;
        }
        public double determinant()
        {
            double[] te = this.elements;
            double n11 = te[0], n12 = te[4], n13 = te[8], n14 = te[12];
            double n21 = te[1], n22 = te[5], n23 = te[9], n24 = te[13];
            double n31 = te[2], n32 = te[6], n33 = te[10], n34 = te[14];
            double n41 = te[3], n42 = te[7], n43 = te[11], n44 = te[15];
            //TODO: make this more efficient
            //( based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm )
            return (
                n41 * (
                    +n14 * n23 * n32
                     - n13 * n24 * n32
                     - n14 * n22 * n33
                     + n12 * n24 * n33
                     + n13 * n22 * n34
                     - n12 * n23 * n34
                ) +
                n42 * (
                    +n11 * n23 * n34
                     - n11 * n24 * n33
                     + n14 * n21 * n33
                     - n13 * n21 * n34
                     + n13 * n24 * n31
                     - n14 * n23 * n31
                ) +
                n43 * (
                    +n11 * n24 * n32
                     - n11 * n22 * n34
                     - n14 * n21 * n32
                     + n12 * n21 * n34
                     + n14 * n22 * n31
                     - n12 * n24 * n31
                ) +
                n44 * (
                    -n13 * n22 * n31
                     - n11 * n23 * n32
                     + n11 * n22 * n33
                     + n13 * n21 * n32
                     - n12 * n21 * n33
                     + n12 * n23 * n31
                )
            );
        }
        public Matrix4 transpose()
        {
            double[] te = this.elements;
            double tmp;
            tmp = te[1]; te[1] = te[4]; te[4] = tmp;
            tmp = te[2]; te[2] = te[8]; te[8] = tmp;
            tmp = te[6]; te[6] = te[9]; te[9] = tmp;
            tmp = te[3]; te[3] = te[12]; te[12] = tmp;
            tmp = te[7]; te[7] = te[13]; te[13] = tmp;
            tmp = te[11]; te[11] = te[14]; te[14] = tmp;
            return this;
        }
        public Matrix4 setPosition(Vector3 v)
        {
            double[] te = this.elements;

            te[12] = v.x;
            te[13] = v.y;
            te[14] = v.z;

            return this;
        }
        public Matrix4 setPosition(double x, double y, double z)
        {
            double[] te = this.elements;

            te[12] = x;
            te[13] = y;
            te[14] = z;
            return this;
        }
        public Matrix4 invert()
        {
            // based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm
            double[] te = this.elements;
            double n11 = te[0], n21 = te[1], n31 = te[2], n41 = te[3],
               n12 = te[4], n22 = te[5], n32 = te[6], n42 = te[7],
               n13 = te[8], n23 = te[9], n33 = te[10], n43 = te[11],
               n14 = te[12], n24 = te[13], n34 = te[14], n44 = te[15],
               t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
               t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
               t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
               t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;
            double det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;
            if (det == 0) return this.set(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            double detInv = 1 / det;
            te[0] = t11 * detInv;
            te[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
            te[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
            te[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;
            te[4] = t12 * detInv;
            te[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
            te[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
            te[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;
            te[8] = t13 * detInv;
            te[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
            te[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
            te[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;
            te[12] = t14 * detInv;
            te[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
            te[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
            te[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;
            return this;
        }
        public Matrix4 getInverse(Matrix4 m, bool throwOnDegenerate = false)
        {
            // based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm
            var te = this.elements;
            var me = m.elements;
            double n11 = me[0], n21 = me[1], n31 = me[2], n41 = me[3],
                n12 = me[4], n22 = me[5], n32 = me[6], n42 = me[7],
                n13 = me[8], n23 = me[9], n33 = me[10], n43 = me[11],
                n14 = me[12], n24 = me[13], n34 = me[14], n44 = me[15],
                t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44,
                t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44,
                t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44,
                t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;
            var det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;
            if (det == 0)
            {
                var msg = "THREE.Matrix4: .getInverse() can't invert matrix, determinant is 0";
                if (throwOnDegenerate == true)
                {
                    throw new Error(msg);
                }
                else
                {
                    console.warn(msg);
                }
                return this.identity();
            }
            var detInv = 1 / det;
            te[0] = t11 * detInv;
            te[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
            te[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
            te[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;
            te[4] = t12 * detInv;
            te[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
            te[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
            te[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;
            te[8] = t13 * detInv;
            te[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
            te[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
            te[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;
            te[12] = t14 * detInv;
            te[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
            te[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
            te[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;
            return this;
        }

        public Matrix4 scale(Vector3 v)
        {
            double[] te = this.elements;
            double x = v.x, y = v.y, z = v.z;
            te[0] *= x; te[4] *= y; te[8] *= z;
            te[1] *= x; te[5] *= y; te[9] *= z;
            te[2] *= x; te[6] *= y; te[10] *= z;
            te[3] *= x; te[7] *= y; te[11] *= z;
            return this;
        }
        public double getMaxScaleOnAxis()
        {
            double[] te = this.elements;
            double scaleXSq = te[0] * te[0] + te[1] * te[1] + te[2] * te[2];
            double scaleYSq = te[4] * te[4] + te[5] * te[5] + te[6] * te[6];
            double scaleZSq = te[8] * te[8] + te[9] * te[9] + te[10] * te[10];
            return JMath.sqrt(JMath.max(scaleXSq, scaleYSq, scaleZSq));
        }
        public Matrix4 makeTranslation(double x, double y, double z)
        {
            this.set(
                1, 0, 0, x,
                0, 1, 0, y,
                0, 0, 1, z,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 makeRotationX(double theta)
        {
            double c = JMath.cos(theta), s = JMath.sin(theta);
            this.set(
                1, 0, 0, 0,
                0, c, -s, 0,
                0, s, c, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 makeRotationY(double theta)
        {
            double c = JMath.cos(theta), s = JMath.sin(theta);
            this.set(
                 c, 0, s, 0,
                 0, 1, 0, 0,
                -s, 0, c, 0,
                 0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 makeRotationZ(double theta)
        {
            double c = JMath.cos(theta), s = JMath.sin(theta);
            this.set(
                c, -s, 0, 0,
                s, c, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 makeRotationAxis(Vector3 axis, double angle)
        {
            // Based on http://www.gamedev.net/reference/articles/article1199.asp
            double c = JMath.cos(angle);
            double s = JMath.sin(angle);
            double t = 1 - c;
            double x = axis.x, y = axis.y, z = axis.z;
            double tx = t * x, ty = t * y;
            this.set(
                tx * x + c, tx * y - s * z, tx * z + s * y, 0,
                tx * y + s * z, ty * y + c, ty * z - s * x, 0,
                tx * z - s * y, ty * z + s * x, t * z * z + c, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 makeScale(double x, double y, double z)
        {
            this.set(
                x, 0, 0, 0,
                0, y, 0, 0,
                0, 0, z, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 makeShear(double xy, double xz, double yx, double yz, double zx, double zy)
        {
            this.set(
                1, yx, zx, 0,
                xy, 1, zy, 0,
                xz, yz, 1, 0,
                0, 0, 0, 1
            );
            return this;
        }
        public Matrix4 compose(Vector3 position, Quaternion quaternion, Vector3 scale)
        {
            double[] te = this.elements;
            double x = quaternion._x, y = quaternion._y, z = quaternion._z, w = quaternion._w;
            double x2 = x + x, y2 = y + y, z2 = z + z;
            double xx = x * x2, xy = x * y2, xz = x * z2;
            double yy = y * y2, yz = y * z2, zz = z * z2;
            double wx = w * x2, wy = w * y2, wz = w * z2;
            double sx = scale.x, sy = scale.y, sz = scale.z;
            te[0] = (1 - (yy + zz)) * sx;
            te[1] = (xy + wz) * sx;
            te[2] = (xz - wy) * sx;
            te[3] = 0;
            te[4] = (xy - wz) * sy;
            te[5] = (1 - (xx + zz)) * sy;
            te[6] = (yz + wx) * sy;
            te[7] = 0;
            te[8] = (xz + wy) * sz;
            te[9] = (yz - wx) * sz;
            te[10] = (1 - (xx + yy)) * sz;
            te[11] = 0;
            te[12] = position.x;
            te[13] = position.y;
            te[14] = position.z;
            te[15] = 1;
            return this;
        }
        public Matrix4 decompose(Vector3 position, Quaternion quaternion, Vector3 scale)
        {
            var ctx = getContext();
            var _v1 = ctx._v1;
            var _m1 = ctx._m1;
            double[] te = this.elements;
            double sx = _v1.set(te[0], te[1], te[2]).length();
            double sy = _v1.set(te[4], te[5], te[6]).length();
            double sz = _v1.set(te[8], te[9], te[10]).length();
            // if determine is negative, we need to invert one scale
            double det = this.determinant();
            if (det < 0) sx = -sx;
            position.x = te[12];
            position.y = te[13];
            position.z = te[14];
            // scale the rotation part
            _m1.copy(this);
            double invSX = 1 / sx;
            double invSY = 1 / sy;
            double invSZ = 1 / sz;
            _m1.elements[0] *= invSX;
            _m1.elements[1] *= invSX;
            _m1.elements[2] *= invSX;
            _m1.elements[4] *= invSY;
            _m1.elements[5] *= invSY;
            _m1.elements[6] *= invSY;
            _m1.elements[8] *= invSZ;
            _m1.elements[9] *= invSZ;
            _m1.elements[10] *= invSZ;
            quaternion.setFromRotationMatrix(_m1);
            scale.x = sx;
            scale.y = sy;
            scale.z = sz;
            return this;
        }
        public Matrix4 makePerspective(double left, double right, double top, double bottom, double near, double far)
        {
            double[] te = this.elements;
            double x = 2 * near / (right - left);
            double y = 2 * near / (top - bottom);
            double a = (right + left) / (right - left);
            double b = (top + bottom) / (top - bottom);
            double c = -(far + near) / (far - near);
            double d = -2 * far * near / (far - near);
            te[0] = x; te[4] = 0; te[8] = a; te[12] = 0;
            te[1] = 0; te[5] = y; te[9] = b; te[13] = 0;
            te[2] = 0; te[6] = 0; te[10] = c; te[14] = d;
            te[3] = 0; te[7] = 0; te[11] = -1; te[15] = 0;
            return this;
        }
        public Matrix4 makeOrthographic(double left, double right, double top, double bottom, double near, double far)
        {
            double[] te = this.elements;
            double w = 1.0 / (right - left);
            double h = 1.0 / (top - bottom);
            double p = 1.0 / (far - near);
            double x = (right + left) * w;
            double y = (top + bottom) * h;
            double z = (far + near) * p;
            te[0] = 2 * w; te[4] = 0; te[8] = 0; te[12] = -x;
            te[1] = 0; te[5] = 2 * h; te[9] = 0; te[13] = -y;
            te[2] = 0; te[6] = 0; te[10] = -2 * p; te[14] = -z;
            te[3] = 0; te[7] = 0; te[11] = 0; te[15] = 1;
            return this;
        }
        public bool equals(Matrix4 matrix)
        {
            double[] te = this.elements;
            double[] me = matrix.elements;
            for (int i = 0; i < 16; i++)
            {
                if (te[i] != me[i]) return false;
            }
            return true;
        }
        public bool equals(IIOArray obj) => equals(obj as Matrix4);
        public Matrix4 fromArray(double[] array, int offset = 0)
        {
            for (int i = 0; i < 16; i++)
            {
                this.elements[i] = array[i + offset];
            }
            return this;
        }
        IIOArray IIOArray.fromArray(double[] array, int offset)
        {
            return fromArray(array, offset);
        }
        public double[] toArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[16];
            double[] te = this.elements;
            array[offset] = te[0];
            array[offset + 1] = te[1];
            array[offset + 2] = te[2];
            array[offset + 3] = te[3];
            array[offset + 4] = te[4];
            array[offset + 5] = te[5];
            array[offset + 6] = te[6];
            array[offset + 7] = te[7];
            array[offset + 8] = te[8];
            array[offset + 9] = te[9];
            array[offset + 10] = te[10];
            array[offset + 11] = te[11];
            array[offset + 12] = te[12];
            array[offset + 13] = te[13];
            array[offset + 14] = te[14];
            array[offset + 15] = te[15];
            return array;
        }
        #endregion

        public override string ToString()
        {
            if (this.elements == null)
                return base.ToString();

            return $"[{string.Join(",", this.elements)}]";
        }
    }

}
